/**
 * @file QWF_Math.h
 * @brief QWF框架数学运算头文件（包含通用数值处理宏函数、单位转换及快速三角函数等功能）
 * @author WeiFan Qiu (qiuweifan0523@faxmail.com)
 * @version 1.0
 * @date 2025-08-07
 * 
 * @copyright Copyright (c) 2025 邱伟凡
 * 
 * @par 修改日志:
 * - 2025-08-07: 初始版本，定义框架基础数学运算宏及函数声明
 * 
 * @bug 待解决问题: 暂无已知问题
 */
#ifndef QWF_MATH_H
#define QWF_MATH_H

#include <stdbool.h>   // 包含布尔类型定义（bool）
#include <stdint.h>    // 包含标准整数类型（如uint32_t）
#include "arm_math.h"  // ARM数学库（提供浮点运算优化函数）

/*****************全用宏函数定义，节省开辟栈区时间，以达到快速运算的效果*****************************/
#define MIN_COVARIANCE 0.000001f  // 最小值（防止计算中除以零的错误，用于协方差等计算场景）

/**************QWF宏函数（数值处理核心逻辑）***************/

/**
 * @brief 限幅函数：将数值限制在指定范围内
 * @param Limiter_Measure 被限幅的数值
 * @param Limiter_Max 上限值
 * @param Limiter_Min 下限值
 * @return 超过上限返回上限值，低于下限返回下限值，否则返回原值
 * @note 常用于电机控制中限制PID输出，防止执行器过驱动
 */
#define QWF_Limiter(Limiter_Measure, Limiter_Max, Limiter_Min)   \
    (((Limiter_Measure) >= (Limiter_Max))                        \
         ? (Limiter_Max)                                         \
         : (((Limiter_Measure) <= (Limiter_Min)) ? (Limiter_Min) \
                                                 : (Limiter_Measure)))

/**
 * @brief 死区函数：消除数值的不敏感区间
 * @param Dead_Meas 被处理的数值
 * @param Dead_Max 死区上限
 * @param Dead_Min 死区下限
 * @return 数值在死区区间内时返回中间值，否则返回原值
 * @note 适用于消除传感器零漂或执行器空行程（如电机启动阈值）
 * @example Dead_Meas=3,Dead_Max=5,Dead_Min=2,返回(5+2)/2=3.5
 */
#define QWF_Dead(Dead_Meas, Dead_Max, Dead_Min)               \
    (((Dead_Meas) >= (Dead_Min) && (Dead_Meas) <= (Dead_Max)) \
         ? (((Dead_Min) + (Dead_Max)) / 2.0f)                 \
         : (Dead_Meas))

/**
 * @brief 最大值比较函数
 * @param QWF_A 比较值A
 * @param QWF_B 比较值B
 * @return 两个数值中的较大值
 */
#define QWF_Max(QWF_A, QWF_B) (((QWF_A) >= (QWF_B)) ? (QWF_A) : (QWF_B))

/**
 * @brief 最小值比较函数
 * @param QWF_A 比较值A
 * @param QWF_B 比较值B
 * @return 两个数值中的较小值
 */
#define QWF_Min(QWF_A, QWF_B) (((QWF_A) <= (QWF_B)) ? (QWF_A) : (QWF_B))

/**
 * @brief 符号判断函数
 * @param QWF_Num 被判断的数值
 * @param QWF_Norm 基准值
 * @return 大于基准值返回1，小于返回-1，等于返回0
 * @note 用于方向判断（如电机正反转控制）
 */
#define QWF_Signed(QWF_Num, QWF_Norm) \
    (((QWF_Num) > (QWF_Norm)) ? 1 : (((QWF_Num) < (QWF_Norm)) ? -1 : 0))

/**
 * @brief 浮点数绝对差值计算
 * @param QWF_A 数值A
 * @param QWF_B 数值B
 * @return 两个浮点数的绝对差（避免直接相减导致符号问题）
 */
#define QWF_Esp(QWF_A, QWF_B) \
    (((QWF_A) >= (QWF_B)) ? ((QWF_A) - (QWF_B)) : ((QWF_B) - (QWF_A)))

/**
 * @brief RPM转rad/s单位转换
 * @param QWF_RPM 转速（转/分钟）
 * @return 弧度/秒（rad/s）
 * @note 公式推导：1 RPM = 2π弧度/60秒 = π/30 rad/s
 */
#define QWF_rpm_to_rad(QWF_RPM) ((QWF_RPM) * PI / 30)

/**
 * @brief 环形循环数值处理（类似DMA乒乓缓冲）
 * @param QWF_Num 当前值
 * @param QWF_Begin 周期起始值
 * @param QWF_End 周期结束值
 * @param QWF_Gap 步长（可正可负）
 * @return 循环后的值（超出范围时从另一端继续）
 * @note 适用于角度循环（如0~360°）或缓冲区索引管理
 * @example QWF_Num=3,QWF_Begin=0,QWF_End=5,QWF_Gap=2,一次依次返回5，2，4，1，3（一个周期内）
 */
#define QWF_Circle(QWF_Num, QWF_Begin, QWF_End, QWF_Gap)      \
    (((QWF_Num) + (QWF_Gap)) > (QWF_End))                     \
        ? (((QWF_Num) + (QWF_Gap)) - (QWF_End) + (QWF_Begin)) \
    : (((QWF_Num) + (QWF_Gap)) < (QWF_Begin))                 \
        ? (((QWF_Num) + (QWF_Gap)) + (QWF_End) - (QWF_Begin)) \
        : ((QWF_Num) + (QWF_Gap))
/**
 * @brief 斜坡函数（缓启动/缓停控制）
 * @param Ramp_Final 目标值
 * @param Ramp_Now 当前值
 * @param QWF_Gap 每步变化量（步长）
 * @return 当前值按步长平滑逼近目标值的结果
 * @note 防止控制量突变导致机械冲击（如摩擦轮加速/减速）
 */
#define QWF_Ramp(Ramp_Final, Ramp_Now, QWF_Gap)                      \
    ((Ramp_Final) >= (Ramp_Now))                                       \
        ? /* 增加阶段 */ ((((Ramp_Now) + (QWF_Gap)) <= (Ramp_Final)) \
                              ? ((Ramp_Now) + (QWF_Gap))             \
                              : (Ramp_Final))                          \
        : /* 减少阶段 */ ((((Ramp_Now) - (QWF_Gap)) >= (Ramp_Final)) \
                              ? ((Ramp_Now) - (QWF_Gap))             \
                              : (Ramp_Final))

/**
 * @brief 浮点数相等判断（考虑精度误差）
 * @param QWF_A 数值A
 * @param QWF_B 数值B
 * @return 两数在1e-6误差范围内相等时返回true
 * @note 避免直接使用==比较浮点数导致的精度问题
 */
#define QWF_Equal(QWF_A, QWF_B) \
    (((QWF_A) >= (QWF_B) - 1E-6f) && ((QWF_A) <= (QWF_B) + 1E-6f))

/**************三角类函数声明（角度处理）***************/
float To_0_T(float Num, float T);  
float Helm_angle(float angle,float v,float T);  
float find_min_angle(float Num_A, float Num_B, int T);
#define find_min_angle_diff(Num_A, Num_B, T) \
    ((Num_B) - find_min_angle((Num_A), (Num_B), (T)))  
/**************快速三角函数（整数角度优化）***************/
extern float fastCos(int angle);  
extern float fastSin(int angle);  

/**************二次方程求解宏（数学计算）***************/

/**
 * @brief 计算二次方程判别式Δ
 * @param a_x2 二次项系数（ax²）
 * @param b_x 一次项系数（bx）
 * @param c 常数项（c）
 * @return Δ = b² - 4ac
 */
#define derta(a_x2, b_x, c) \
    ((b_x) * (b_x) - 4 * (a_x2) * (c))  

/**
 * @brief 二次方程较大根计算（x=(-b+√Δ)/(2a)）
 * @param a_x2 二次项系数
 * @param b_x 一次项系数
 * @param c 常数项
 * @return 较大的根（仅当Δ≥0时有效）
 */
#define x2_func_x_plus(a_x2, b_x, c) \
    (-(b_x) + sqrt(derta(a_x2, b_x, c))) / (2 * (a_x2))

/**
 * @brief 二次方程较小根计算（x=(-b-√Δ)/(2a)）
 * @param a_x2 二次项系数
 * @param b_x 一次项系数
 * @param c 常数项
 * @return 较小的根（仅当Δ≥0时有效）
 */
#define x2_func_x_sub(a_x2, b_x, c) \
    (-(b_x) - sqrt(derta(a_x2, b_x, c))) / (2 * (a_x2))

/**************时间获取宏（依赖HAL库）***************/

/**
 * @brief 获取当前系统时间（毫秒级）
 * @return STM32 HAL库的系统滴答时间（ms）
 * @note 依赖STM32 HAL库，其他平台需修改实现
 */
#define get_tick_ms() 	HAL_GetTick()           

/**
 * @brief 获取当前系统时间（秒级，浮点型）
 * @return 系统时间（s）
 */
#define get_tick_s() 	HAL_GetTick() / 1000.0f  

#endif
